#include "calculoAutovectores.h"

//esta funcion no se usa, se hizo solo para testear
Matriz* calcularAutovectores(Matriz* original,double tolerancia,int maxIter)
{
	Matriz temp(original->cantidadDeFilas(),original->cantidadDeFilas());
	temp.copiarMatriz(original);
	metodoHouseHolder(&temp);
	Matriz* autovalores = autovalores_qr(temp,tolerancia,maxIter);
	Matriz* ret = autovector(*original,*autovalores,tolerancia);
	delete autovalores;
	return ret;
}

//tridiagonaliza la matriz simetrica
void metodoHouseHolder(Matriz* original)
{
	Tam columnas = original->cantidadDeColumnas();
	float elQ,elemento,suma,elPROD,elRSQ,zporv,vporz,elAlfa;
	Matriz* vectorV=new Matriz(columnas,1);
	Matriz* vectorU=new Matriz(columnas,1);
	Matriz* vectorZ=new Matriz(columnas,1);
	for(Tam k=1;k<columnas-1;k++)
	{
		elQ=0;
		elemento=original->get(k+1,k);
		
		//calculo del q PASO 2
		for(Tam j=k+1;j<=columnas;j++)
			elQ+=pow(original->get(j,k),2);
		
		//calculo del alfa PASO 3
		elAlfa=(elemento==0 ? -sqrt(elQ) : - sqrt(elQ)*elemento/fabs(elemento));
		//calculo de RSQ PASO 4
		elRSQ=pow(elAlfa,2) - elAlfa*elemento;

		//calculo del v PASO 5
		/*
		for(Tam t=1;t<k;t++)
			vectorV->set(t,1,0.0);
		*/
		vectorV->set(k,1,0.0);
		vectorV->set(k+1,1,elemento - elAlfa);
		
		for(Tam j=k+2;j<=columnas;j++)
			vectorV->set(j,1,original->get(j,k));
		
		//calculo del u PASO 6
		/*
		for(Tam t=1;t<k;t++)
		vectorU->set(t,1,0);
		*/
		for(Tam j=k;j<=columnas;j++)
		{
			suma=0;
			for(Tam i=k+1;i<=columnas;i++)
				suma+=original->get(j,i)*vectorV->get(i,1);
			vectorU->set(j,1,suma/elRSQ);
		}
		
		//calculo del PROD PASO 7
		elPROD=0;
		for(Tam j=k+1;j<=columnas;j++)
			elPROD+=vectorU->get(j,1)*vectorV->get(j,1);
		
		//calculo del z PASO 8
		/*
		for(Tam t=1;t<k;t++)
			vectorZ->set(t,1,0);
		*/
		for(Tam j=k;j<=columnas;j++)
			vectorZ->set(j,1,vectorU->get(j,1) - (elPROD/(2*elRSQ))*vectorV->get(j,1));
		
		//PASO 9
		for(Tam l=k+1;l<columnas;l++)
		{
			//PASO 10
			for(Tam j=l+1;j<=columnas;j++)
			{
				vporz=vectorV->get(l,1)*vectorZ->get(j,1);
				zporv=vectorZ->get(l,1)*vectorV->get(j,1);
				original->set(j,l,original->get(j,l) - vporz - zporv);
				original->set(l,j,original->get(j,l));
			}
			//PASO 11
			original->set(l,l,original->get(l,l) - 2*vectorV->get(l,1)*vectorZ->get(l,1));
		}
		
		//PASO 12
		vporz=2*vectorV->get(columnas,1)*vectorZ->get(columnas,1);
		original->set(columnas,columnas,original->get(columnas,columnas) - vporz);
		
		//PASO 13
		for(Tam j=k+2;j<=columnas;j++)
		{
			original->set(k,j,0.0);
			original->set(j,k,0.0);
			
		}
		
		//PASO 14
		vporz=vectorV->get(k+1,1)*vectorZ->get(k,1);
		original->set(k+1,k,original->get(k+1,k) - vporz);
		original->set(k,k+1,original->get(k+1,k));
	}
	
	delete vectorU;
	delete vectorV;
	delete vectorZ;
}

//calcula los autovectores y autovalores
Matriz* autovalores_qr(const Matriz& m, double tolerance, int max_iteration)
{
	int k= 0, n= m.cantidadDeColumnas(), iRet= 1;
	vector<double>a(n),b(n);
	Matriz* ret=new Matriz(n,1);
        
        
	a[0]= m.get(1,1);
        
	for (int i = 1; i < n; ++i) {
		a[i]= m.get(i+1, i+1);
		b[i]= m.get(i+1, i);
	}
        
	double shift= 0;
        
	while(k < max_iteration){

		if (fabs(b[n-1])<tolerance){
			ret->set(iRet, 1, a[n-1] + shift);
			iRet++;
			n--;
		}
                
		if (fabs(b[1])<tolerance){
			ret->set(iRet, 1, a[0] + shift);
			iRet++;
			n--;
			a[0]= a[1];
			for (int i= 1; i < n; i++) {
				a[i]=a[i+1];
				b[i]=b[i+1];
			}
		}
                
		if (n == 0){
			ordenarPorValorAbsoluto(ret);
			return ret;
		}
                
		if (n == 1){
			ret->set(iRet, 1, a[0] + shift);
			iRet++;
			ordenarPorValorAbsoluto(ret);
			return ret;
		}
                
		double rb= -(a[n-2] + a[n-1]);
		double rc= (a[n-1] * a[n-2]) - (pow(b[n-1],2));
		double rd= sqrt(pow(rb,2)-4*rc);
		double mu1= 0, mu2= 0;
                
		if (rb>0){
			mu1= (-2*rc) / (rb+rd);
			mu2= -(rb+rd) / 2;
		}
		else{
			mu1= (rd-rb) / 2;
			mu2= (2*rc) / (rd-rb);
		}
                
		if (n==1){
			ret->set(iRet, 1, mu1 + shift);
			iRet++;
			ret->set(iRet, 1, mu2 + shift);
			iRet++;
		}
                
		double s= min(fabs(mu1-a[n-1]), fabs(mu2-a[n-1]));
		shift+= s;
                
		vector<double> vd(n), vx(n), vy(n);
		vector<double> vz(n), vc(n), vs(n);
		vector<double> vq(n), vr(n);
                
		for (int j = 0; j < n; ++j) 
			vd[j]= a[j]-s;
                
		vx[0]= vd[0];
		vy[0]= b[1];
                
		for (int j = 1; j < n; ++j) {
			vz[j-1]= sqrt(pow(vx[j-1],2) + pow(b[j],2));
			vc[j]= vx[j-1]/vz[j-1];
			vs[j]=b[j]/vz[j-1];
                        
			vq[j-1]= (vc[j]*vy[j-1]) + (vs[j]*vd[j]);
			vx[j]= -(vs[j]*vy[j-1]) + (vc[j]*vd[j]);
                        
			if (j!=n-1){
				vr[j-1]= vs[j]*b[j+1];
				vy[j]= vc[j]*b[j+1];
			}
		}
                
		vz[n-1]= vx[n-1];
		a[0]= vs[1]*vq[0] + vc[1]*vz[0];
		b[1]= vs[1]*vz[1];
		
		for (int j = 1; j < n-1; j++) {
			a[j]= vs[j+1] * vq[j] + vc[j] * vc[j+1] * vz[j];
			b[j+1]= vs[j+1] * vz[j+1];
		}
		a[n-1]= vc[n-1] * vz[n-1];
		k++;
	}
	ordenarPorValorAbsoluto(ret);
	return ret;
}

Matriz* autovector(const Matriz& mat,const Matriz& aValores,double tolerance){
   Tam n=mat.cantidadDeFilas();
   Matriz copia(n,n);
   Matriz* ret= new Matriz(n,n);
   Matriz vectorNulo(n,n);
   for(Tam inAuto=1;inAuto<=n;inAuto++){
       copia.copiarMatriz(&mat);

       for(Tam q=1;q<=n;q++)
           copia.set(q,q,copia.get(q,q)-aValores.get(inAuto,1));


       vector<int> arrSwap(n);
       double coef;
       Indice i, j, k, filaMax;
       //Eliminacion Gaussiana con pivoteo
       for(k=1;k<=copia.cantidadDeColumnas(); k++){
           filaMax= copia.getFilaMaxElemento(k,k);
           copia.swapFila(k,filaMax);

           if (copia.get(k,k) !=0){
               for(i=k+1;i<=copia.cantidadDeFilas();i++){
                   coef= copia.get(i,k) / copia.get(k,k);
                   copia.set(i,k,0);
                   for(j=k+1;j<=copia.cantidadDeColumnas();j++)
                       copia.set(i,j,copia.get(i,j) - copia.get(k,j)*coef);
               }
           }
       }

       //Solucion
       ret->set(n,inAuto,1);
       for(k=copia.cantidadDeFilas()-1; k>0; k--){
           for(i=k+1; i<=copia.cantidadDeColumnas(); i++)
               ret->set(k,inAuto, ret->get(k,inAuto) -(copia.get(k,i)*ret->get(i,inAuto)));
           if (copia.get(k,k)!=0)
               ret->set(k,inAuto,ret->get(k,inAuto)/copia.get(k,k));
       }
   }
   normalizarPorColumnas(ret);
   return ret;
}

void normalizarPorColumnas(Matriz* mat)
{
	double norma;
	for(Tam j=1;j<=mat->cantidadDeColumnas();j++)
	{
		norma=0;
		for(Tam i=1;i<=mat->cantidadDeFilas();i++)
			norma+=pow(mat->get(i,j),2);
		norma=sqrt(norma);
		for(Tam i=1;i<=mat->cantidadDeFilas();i++)
			mat->set(i,j,mat->get(i,j)/norma);
	}
}

void ordenarPorValorAbsoluto(Matriz* vector)
{
	quicksort(vector,1,vector->cantidadDeFilas());
}

void quicksort(Matriz* vector,Tam iz,Tam der)
{
	if (iz < der)
	{
		int iz_aux(iz), der_aux(der);
		double aux = fabs(vector->get(iz,1));
	 
		while (iz_aux != der_aux)
		{
			while (fabs(vector->get(iz_aux,1)) > aux) ++iz_aux;
			while (aux > fabs(vector->get(der_aux,1))) --der_aux;
			double temp=vector->get(iz_aux,1);
			vector->set(iz_aux,1,vector->get(der_aux,1));
			vector->set(der_aux,1,temp);
		}
		quicksort(vector, iz, iz_aux - 1);
		quicksort(vector, iz_aux + 1, der);
	}
}
